
#include <sys/types.h>
#include <stdexcept>
#include <stdio.h>     /* Standard input/output definitions */
#include <stdlib.h>
#include <string.h>    /* String function definitions */
#include <unistd.h>    /* UNIX standard function definitions */
#include <fcntl.h>     /* File control definitions */
#include <errno.h>     /* Error number definitions */
#include <termios.h>   /* POSIX terminal control definitions */
#include <sys/ioctl.h>
#include <getopt.h>
#include <sys/select.h>
#include <sys/time.h>
#include <unistd.h>
#include <iostream>

#include <talos/arduino/Arduino.hpp>

namespace talos
{

Arduino::Arduino( const std::string serialport, const int baudrate )
{
	struct termios toptions;

	m_fd_serial = open( serialport.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );

	if ( m_fd_serial == -1 )
	{
		throw std::invalid_argument( std::string( "Unable to open port : " ) + serialport );
	}

	if ( tcgetattr( m_fd_serial, &toptions ) < 0 )
	{
		throw std::runtime_error( "Couldn't get termios attributes" );
	}

	// let you override switch below if needed
	speed_t brate = baudrate;

	switch( baudrate )
	{
		case 4800:   brate=B4800;   break;
		case 9600:   brate=B9600;   break;
#ifdef B14400
		case 14400:  brate=B14400;  break;
#endif
		case 19200:  brate=B19200;  break;
#ifdef B28800
		case 28800:  brate=B28800;  break;
#endif
		case 38400:  brate=B38400;  break;
		case 57600:  brate=B57600;  break;
		case 115200: brate=B115200; break;
	}

	cfsetispeed( &toptions, brate );
	cfsetospeed( &toptions, brate );

	// 8N1
	toptions.c_cflag &= ~PARENB;
	toptions.c_cflag &= ~CSTOPB;
	toptions.c_cflag &= ~CSIZE;
	toptions.c_cflag |= CS8;

	// no flow control
	toptions.c_cflag &= ~CRTSCTS;

	// turn on READ & ignore ctrl lines
	toptions.c_cflag |= CREAD | CLOCAL;

	// turn off s/w flow ctrl
	toptions.c_iflag &= ~( IXON | IXOFF | IXANY );

	// make raw
	toptions.c_lflag &= ~( ICANON | ECHO | ECHOE | ISIG );

	// make raw
	toptions.c_oflag &= ~OPOST;

	// see: http://unixwiz.net/techtips/termios-vmin-vtime.html
	toptions.c_cc[VMIN]  = 0;
	toptions.c_cc[VTIME] = 20;

	if( tcsetattr( m_fd_serial, TCSANOW, &toptions ) < 0 )
	{
		throw std::runtime_error( "Couldn't set termios attributes" );
	}

	m_listening = talos::Functor< Arduino >( this, &Arduino::listenSerial, NULL );
	m_listening.startThreaded();
}

int Arduino::writeByte( uint8_t b )
{
	pthread_mutex_lock( &m_serial_lock );
	int n = write( m_fd_serial, &b, 1 );
	pthread_mutex_unlock( &m_serial_lock );

	if( n != 1 ) return -1;

	return 0;
}

int Arduino::writeString( const std::string str )
{
	int len = strlen( str.c_str() );

	pthread_mutex_lock( &m_serial_lock );
	int n = write( m_fd_serial, str.c_str(), len );
	pthread_mutex_unlock( &m_serial_lock );

	if( n != len )
		return -1;

	return 0;
}

void* Arduino::listenSerial( void* )
{
	fd_set read_set;
	std::string res;

	while (true)
	{
		FD_ZERO( &read_set );
		FD_SET( m_fd_serial, &read_set );
		select( m_fd_serial + 1, &read_set, NULL, NULL, NULL );

		if ( FD_ISSET( m_fd_serial, &read_set ) )
		{
			char b[1];
			pthread_mutex_lock( &m_serial_lock );
			int n = read( m_fd_serial, b, 1 );
			pthread_mutex_unlock( &m_serial_lock );

			res.append( 1, b[0] );

			if ( n < 1 )
			{
				std::cerr << "Error while listening serial port." << std::endl;
				continue;
			}

			processMessage( res );
		}
	}

	return NULL;
}

}

